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Abstract:  Improvements  in  high  resolution  small  forward  looking  sonar  (FLS)  and 
computer  processing  have  made  it  possible  to  develop  an  obstacle  avoidance  system 
(OAS)  for  small  diameter  Autonomous  Underwater  Vehicles  (AUV).  An  AUV  with  such 
a  system  can  maneuver  around  unanticipated  obstacles  that  may  be  proud  of  the  ocean 
floor.  This  ability  can  prevent  serious  damage  to  the  vehicle  or  the  environment.  This 
paper  discusses  developments  in  control  and  computer  vision  techniques  of  an  OAS 
designed  to  vertically  avoid  obstacles  found  on  the  ocean  floor.  Results  are  presented 
from  recent  in-water  testing.  Copyright  ©  2007  IF  AC 
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1.  INTRODUCTION 

We  discuss  progress  in  a  developmental  OAS 
mounted  on  the  front  of  a  small  AUV.  The  scope  of 
the  problem  is  limited  by  considering  the  avoidance 
of  obstacles  proud  of  the  ocean  floor.  The  maneuver 
around  the  obstacle  is  in  the  vertical  plane.  In  other 
words,  if  an  obstacle  is  detected  in  the  path  of  the 
AUV,  the  AUV  will  maneuver  up  over  the  obstacle 
and  return  to  the  pre-planned  altitude.  Frequently 
AUVs  conduct  side  scan  sonar  surveys  of  the 
bottom,  these  surveys  are  conducted  close  to  the 
ocean  floor  to  limit  gaps  in  sonar  coverage.. 

The  discussion  focuses  around  the  image  processing 
and  the  control  necessary  to  avoid  an  unanticipated 
obstacle.  This  is  a  continuation  of  the  work  first 
reported  in  OCEANS  2005  [Horner].  The  image 
processing  section  focuses  on  tracking  the  ocean 
floor  using  the  forward  looking  sonar.  Using  the 
vehicle  state  together  with  a  hough  space 
representation  of  the  image,  an  Extended  Kalman 
Filter  (EKF)  is  constructed  to  give  a  predictive 
estimate  where  the  ocean  floor  is  in  the  sonar  image 
space  [Mills].  Once  the  ocean  floor  is  reliably 
tracked,  obstacles  and  features  can  be  tracked  that 
are  proud  of  the  ocean  floor.  When  an  obstacle  is 
high  enough  off  the  ocean  floor  to  be  a  threat  to  the 
vehicle,  the  pre-planned  vehicle  path  is  modified. 

A  direct  method  for  calculating  real-time,  near 
optimal  polynomial  trajectories  [Yakimenko]  is 
presented  for  the  avoidance  behavior.  The 
calculation  takes  into  consideration  an  optimization 
function  based,  in  part,  on  the  idea  of  minimizing  the 
altitude  change  to  ensure  that  sensor  coverage  stays 
consistent.  The  real  time  trajectory  planning  has  the 
secondary  benefit  of  being  able  to  react  to  additional 
obstacles  that  may  have  been  occluded  by  the  first 
identified  obstacle. 


The  paper  is  organized  as  follows:  Section  two 
describes  the  obstacle  avoidance  framework.  Section 
three  gives  a  description  of  the  sonar.  Section  four 
discusses  sonar  image  processing.  Section  five 
discusses  the  AUV  motion  model.  Section  six 
discusses  optimal  problem  formulation  and  Section 
seven  describes  near  optimal  trajectory  generation 
and  Section  eight  describes  recent  results. 

2.  FRAMEWORK 

The  obstacle  avoidance  framework  (Fig.  1)  consists 
of  the  environmental  map,  a  planning  module,  a 
localization  module  and  the  sensors  and  actuators  for 
the  control  of  the  robot.  The  environmental  map  is 
the  world  according  to  the  robot.  This  can  include  a 
priori  knowledge,  for  example  the  positions  of 
charted  underwater  obstacles.  It  takes  as  input  the 
sensor  and  state  information  from  the  robot  and 
translates  the  sensor  information  into  the  global 
space.  The  localization  module  takes  as  input  the 
most  current  position  estimate  from  the  robot’s 
navigation  model  and  select  features  from 
environmental  model  to  provide  (if  possible)  a  better 
estimate  of  the  robots  position. 

The  planning  module  is  responsible  for  coming  up 
with  a  trajectory  that  the  robot  can  follow  that  is  free 
of  obstacles  and  in  some  sense  optimal.  It  consists  of 
the  deliberative  and  reactive  planners.  They  take 
information  from  the  environment  and  localization 
modules  to  produce  the  trajectory  that  the  robot 
controller  tracks.  The  deliberative  planner  uses  the 
environmental  map  to  produce  trajectories  to  a  goal 
state  that  is  free  of  obstacles.  The  reactive  planner 
takes  recent  sensor  information  to  produce  a 
trajectory  which  will  avoid  an  unanticipated  obstacle 
but  provides  no  guarantees  of  reaching  the  end  goal 
state. 
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3.  SONAR  DESCRIPTION 

The  forward  looking  sonar  is  a  Blazed  Array  450 
KHz  prototype  built  by  Blueview  Technologies.  The 
system  has  a  total  of  six  staves,  four  oriented 


Figure  1.  Obstacle  Avoidance  Framework 

horizontally  and  two  oriented  vertically.  Each 
individual  stave  has  an  approximate  23  degrees  field 
of  view  with  12  degrees  of  vertical  aperture.  The 
range  resolution  is  ,092m  and  the  angular  resolution 
is  a  nominal  1.2  degree.  The  staves  oriented 
horizontally  combine  together  create  a  single  sonar 
image.  This  image  has  a  90  degree  field  of  view  with 
12  degrees  of  vertical  ambiguity.  The  staves  oriented 
vertically  combine  to  produce  a  single  image.  This 
image  has  a  46  degree  field  of  view  and  has  12 
degrees  of  horizontal  ambiguity.  For  this  paper,  only 
the  vertical  images  are  considered.  Figure  2  is  an 
example  of  a  vertical  image. 

4.  SONAR  IMAGE  PROCESSING 

This  section  describes  some  of  the  techniques  used  to 
detect  and  track  an  obstacle  using  the  vertically 
mounted  FLS  arrays. 

4. 1  Detection  of  the  Ocean  Floor 

The  primary  technique  used  to  detect  the  ocean  floor 
is  the  Hough  Transform  [Hough].  It  is  a  common 
technique  in  image  processing  for  the  identification 
of  lines  from  a  set  of  2D  points  [Duda  and  Hart].  In 
mobile  robotics,  the  Hough  Transform  (HT)  has  been 
used  for  a  range  of  applications  including  vision 
based  self-localization  [Iocchi  and  Nardi].  The  HT 
uses  the  parameterized  line  equation 

p  =  xcos#  +  ysinO  (1) 

where  0  is  the  angle  of  the  line  to  the  x-axis,  p  is  the 
perpendicular  distance  from  the  line  and  the  origin  of 
a  selected  point  in  the  image  space,  and  x  and  y 
represent  a  point  on  the  line.  A  transformation  from 
the  original  image  to  a  hough  image  converts  the 
( x,y )  image  points  into  the  (p,9)  hough  space.  A 
line  in  the  image  space  is  represented  as  a  point  in 
the  hough  space.  In  this  case,  detection  of  the  ocean 


floor  is  accomplished  by  searching  for  a  point  in  the 
hough  space. 
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Figure  2.  Example  of  a  vertical  sonar  image  with  a 
obstacle  located  approximately  35M  from  the  AUV 

Consistent  tracking  of  the  ocean  floor  is  made  easier 
by  taking  into  account  the  state  of  the  vehicle  and 
using  this  information  to  track  the  position  of  the 
ocean  floor  in  the  hough  image  space.  Specifically, 
an  EKF  has  been  used  to  estimate  where  the  point 
(i.e.  the  ocean  floor)  should  be  in  the  hough  space 
given  the  current  AUV  position  and  a  motion  model. 

The  EKF  takes  as  input  AUV  pitch,  roll,  heading, 
heading  rate,  surge,  and  heave,  altitude  and  ocean 
floor  point  in  the  hough  space  ip.  9)  and  produces  a 
predictive  estimate  of  ocean  floor  in  the  hough  space. 
This  is  converted  back  into  the  image  space  and  is 
used  as  the  reference  line  in  the  search  for  obstacles. 

4.2  Detection  of  Obstacles 

In  previous  work,  using  a  model  for  the  motion  of  the 
REMUS  AUV  an  obstacle  was  determined  to  be  a 
collision  threat  to  the  AUV  when  the  leading  edge 
slope  of  the  obstacle  is  greater  than  a  nominal  45 
degree  angle  to  the  ocean  floor.  This  is  the  angle  at 
which  the  Doppler  Velocity  Log  (DVL)  reporting  at 
2Hz  can  no  longer  provide  altitude  data  quickly 
enough  for  the  vehicle  control  to  overcome  the 
altitude  change.  This  is  used  as  the  template  for 
determining  when  to  activate  an  avoidance  behavior. 

With  each  image,  the  ocean  floor  is  first  localized. 
Next  a  search  box  is  constructed  for  the  relevant  area 
forward  of  the  AUV.  The  area  is  searched  using  the 
cvBlobsLib  library1.  When  an  object  is  detected  a 
threshold  number  of  times  and  is  a  threat  to  the 
vehicle  it  is  classified  as  an  obstacle. 

The  Hough  EKF  also  helps  in  separating  out 
obstacles  in  the  sonar  image.  By  tracking  the  ocean 
floor  in  the  hough  space  obstacles  can  be  separated 
from  the  ocean  floor  by  the  angle  of  the  obstacle  in 
the  image  space  and  the  distance  from  the  image 
center  p  .  Figure  3  shows  the  original  image  (from 
Figure  2)  in  the  hough  space.  Notice  the  obstacle  is 
visible  as  a  separate  curved  line  where  the  peak 


1  http://opencvlibrary.sourceforge.net/cvBlobsLib 


intensity  is  at  the  approximate  coordinate 
(/?  =  111,(9  =  179). 
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Figure  3.  Hough  representation  of  the  vertical  sonar 
image  shown  above. 

5.  AUV  MODEL 


This  section  describes  the  AUV  model  used  by  the 
reactive  trajectory  planner.  Consider  the  two- 
dimensional  kinematics  equation  of  motion  for  the 
vertical  channel  for  the  AUV. 


x  =  wsin(9  +  MCOs<9 
z  =  w  cos  (9 -m  sin# 


(2) 


6.  OPTIMAL  PROBLEM  FORMULATION 

The  next  step  is  to  formulate  the  general  optimization 
problem  as  applied  to  the  problem  at  hand.  We  post  it 
as  follows. 

There  is  set  of  admissible  trajectories 

z (t)  =  [x(t),z(t),w(t),q(t),0{t)]T  e  S , 

S  =  {z(/)  e  Z5  <=  is5},  te[t0,t/] 

that  satisfies  a  set  of  ordinary  differential  equations 


(2)-(3) 

Z;=f/(Z,4,C) 

«  =  1, 2,. ..,5 

where  4(0  *s  the 

only  control  and 

c  =  {cuc2,...,c  ,},c  e  CP  is  the  vector  of  AUV, 
characteristics  with  initial  conditions  (z(t0),  Ss(t0)) , 
terminal  conditions  (z (tf),Ss(tf))  and  the  following 

constraints  on  state  space  controls  and  control 
derivatives  (listed  in  order): 

i\(t,z)  =  {rjl(t,z),Tj2(t,z),...,TjJt,z)}1  >0 

Ut,z)  =  {£(t,  z,Ss),  4(t,  z ,4), ..,  £(f,  z,  4)f  >  0  (6) 

n(t,  z)  =  (t,  z,  4 ),  x2  (t,  z,  4 ), .. .,  7rv(t,  z,  4  )f  >  0 


where  x  is  the  AUV  longitudinal  position,  z  is  its 
altitude  with  respect  to  the  ocean  floor,  u  and  w  are 
the  forward  and  heave  velocities  respectfully  and  8  is 
the  pitch  angle.  Next  consider  the  dynamics  equation 
for  the  REMUS  AUV  for  vertical  motion  using  the 
standard  state-space  representation  with  the  state 
vector  xv  (t)  =  [w,  q,0]T  : 


The  problem  is  to  find  an  optimal  trajectory  and 
optimal  control  4  op,(0  that  minimizes  some 
performance  index,  for  instance  some  integral  index 

‘f 

J  =  \f0(t’z’ds)dt  (V) 

*0 


xv(0  =  avxv(0  +  bv4(0  (3) 


Here  q  is  the  pitch  rate  and  Ss  is  the  dive  plane 
deflection,  the  matrices  A  and  B  (4)  are  the 
multiplication  of  the  inverse  of  the  mass  matrix  with 
the  motion  model.  The  derivation  of  the  matrices  A 
and  B  and  the  coefficients  used  for  the  forces, 
moments  and  mass  moments  of  inertia  are  taken 
from  [Healey]. 


w-Z„.  -Z. 

0" 

-1 

~z 

m^+Zcl 

0 

4  = 

-K 

0 

Mw 

H 

zBB-z<m 

0  0 

1 

0 

1 

0 

m-Z  — Z. 

0" 

-1 

"V 

Bv= 

-K 

0 

Ms, 

0  0 

1 

0 

(4) 


For  an  AUV  constraints  (8)  result  in  1)  vertically 
avoiding  the  obstacle,  2)  staying  within  the 
constraints  on  the  dive  plane  deflection  Ss,  and  3) 
accounting  for  plane  deflection  dynamics  (modeling 
as  a  first-order  system): 

'7(z )  =  z(xobs)-zobs  >0 

£(4)  =  4max-|4(0|^0  (8) 

^(4)  =  4max-|4(0|>0 

As  a  performance  index  (9)  it  is  natural  to  use  the 
combined  one  that  1)  minimizes  the  distance  traveled 
to  avoid  the  obstacle,  and  2)  maximizes  the  amount 
of  time  with  the  pitch  of  the  AUV  between  nominal 
6nom  of  0  and  -3  degrees: 


V 

j  =  \[{z{t)-z0f+km)-e„om)2]dt 


where  k  is  some  scaling  coefficient  (of  the  order  of 
11).  This  later  one  is  to  try  and  ensure  the  sonar  is 
looking  forward  a  maximum  amount  of  time  during 
the  avoidance  maneuver. 


7.  NEAR  OPTIMAL  CONTROL  FOR  REACTIVE 
OBSTACLE  AVOIDANCE 

When  an  obstacle  is  identified  by  the  image 
processing  routine,  an  OA  behavior  is  activated  and 
the  position  and  maximum  height  of  the  obstacle  is 
sent  to  this  process.  Originally  both  with  the  NPS 
ARIES  and  REMUS  AUVs  reactive  OA  behavior 
was  achieved  by  applying  an  additive  Gaussian 
altitude  curve  over  the  obstacle  position.  This  proved 
to  be  an  adequate  first  solution  but  had  two 
detractions.  First,  any  obstacle  proud  of  the  ocean 
floor  ensonified  by  the  FLS,  may  occlude  any  objects 
behind  it.  Using  a  Gaussian  additive  altitude 
approach  the  AUV  runs  the  risk  of  navigating  into 
another  obstacle  on  the  “backside”  of  the  Gaussian 
curve.  Second,  the  implementation  doesn’t  permit  an 
AUV  to  react  to  moving  objects. 


The  degree  of  the  polynomial  M  is  determined  by  the 
number  of  boundary  conditions  that  must  be 
satisfied.  In  our  case  the  desired  trajectory  includes 
constraints  on  the  initial  and  final  position,  velocity 
and  acceleration.  In  this  case  the  minimal  order  of  the 
polynomial  is  5.  However,  to  increase  the  number  of 
varied  parameters  and  allow  for  more  flexibility  of 
the  candidate  trajectory  the  order  of  polynomials  can 
be  higher  than  5.  Say  we  go  with  the  seventh-order 
polynomial,  so  that 


k- 3 


X\T)  =  2^aikT 

k= 3 

7  1 


<(r)  =  I 


*/(*■) =x 


max(l,A:-2) 
7  1 


-a,,T 


*= i  max(l,(£-l)(£-2)) 

*,(r  )  =  S(maX(1^3))V‘. 


k=0 


k\ 


(ii) 


Instead  the  approach  is  to  use  near  optimal  real-time 
polynomial  trajectory  path  planner  for  reactive 
obstacle  avoidance.  The  trajectory  generation  and 
optimization  procedures  [Yakimenko]  are  now 
described  for  AUV  navigation.  This  direct-method- 
based  procedure  assures  the  following: 

-  the  boundary  conditions  including  high-order 
derivatives  are  satisfied  a  priori, 

-  the  control  commands  are  smooth  and 
physically  realizable, 

-  the  method  is  very  robust  and  is  not  sensitive 
to  small  variations  in  the  input  parameters; 

-  only  a  few  variable  parameters  are  used,  thus 
ensuring  that  the  iterative  process  during 
optimization  converges  well  and  that  the 
continuous  update  of  the  solution  allows 
reliable  path  following  even  with  no  standard 
feedback. 

Another  important  feature  of  this  method  is  that  it 
allows  handling  any  compound  performance  index, 
like  in  (11).  Therefore,  we  are  not  limited  to  simple 
standard  indices  like  time,  fuel  expenditure,  etc. 

7. 1  Generating  a  Candidate  Trajectory’ 

Consider  a  desired  AUV  trajectory  as 
p(r)  =  [x(r),z(r)]r  where  T  is  some  abstract 
argument,  the  virtual  arc,  and  is  defined  by 
t  =  [0;r/  ] ,  where  r,  is  the  total  length  of  the  virtual 

arc.  Each  coordinate  X,,  i  =  1,2  (for  simplicity  of 
notation  we  assume  xl (t)  =  x(t)  and  x,(r)  =  z(r) ) 
is  represented  by  a  polynomial  of  degree  N  with  the 
form: 


aikr  , 


i  =  l,2 


(10) 


The  coefficients  aik ,  i- 1,2,  k  =  0,...,7  can  be 
determined  by  solving  the  following  system  of  linear 
algebraic  equations 
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As  seen,  no  matter  what  value  of  the  final  arc  r f  we 

have,  the  boundary  conditions  will  be 
unconditionally  satisfied.  But  varying  this  value 
allows  varying  the  shape  of  the  candidate  trajectory. 
Figure  4  demonstrates  an  example  when  an  AUV 
navigates  at  3m  altitude  at  1.5m/s  and  in  order  to 
avoid  some  obstacle  has  to  perform  a  pop-up 
maneuver.  Changing  the  value  of  r f  allows  avoiding 
obstacles  with  different  height. 


Figure  4.  Variances  of  the  candidate  trajectory  while 
changing  the  value  of  t  f . 

Now  what  about  four  free  parameters  which  in  our 
case  are  components  of  the  initial  and  final  jerk,  x” 


and  x'" ,  i  =  1,2  (15)?  Having  these  additional  varied 

parameters  allows  to  further  change  the  overall  shape 
of  the  trajectory  to  be  able  to  vary  the  combined 
performance  index  (11)  we  chose.  To  this  end  Fig. 5 
shows  an  example  of  an  AUV  navigating  at  3m 
altitude  at  1.5m/s  and  trying  to  avoid  a  6m  obstacle 
located  in  between  initial  and  final  points. 


Figure  5.  Symmetric  varying  of  x"0  and  x['f  to  avoid 
an  obstacle  with  optimizing  r  f . 

Now,  let  us  address  the  reason  for  choosing  some 
abstract  parameter  T  (not  time,  not  path  length)  as 
an  argument  for  the  reference  functions  (12)-(13). 
Assume  for  a  moment  that  T  =  t .  In  this  case  once 
we  defined  the  trajectory  we  unambiguously  defined 
the  speed  profile  along  this  trajectory  as  well  since 

V(t)  =  4u2  +  w2  =  ^x(t)2  +  z(t)2  (13) 


Figure  6.  Example  of  the  dynamic  reconfiguration  of 
the  trajectory. 

7.2  Inverse  Dynamics 

Having  the  candidate  trajectory  determined  by  (13)- 

(15)  the  goal  now  is  use  inverse  dynamics  with  the 
AUV  model  to  calculate  the  remaining  states  and 
dive  plane  commands  that  is  required  to  follow  this 
candidate  trajectory.  In  this  way  we  can  ensure  that 
the  candidate  trajectory  does  not  exceed  vehicle  or 
configuration  space  constraints.  Using  the  relation 

(16)  for  any  parameter  C,  , 

«r)  =  fy  =  r(^(t)  (16) 

dr  at 

and  AUV  motion  model  the  it  is  possible  to  convert 
the  equations  of  motion  into  the  T  domain 


Obviously  we  cannot  allow  this  and  would  want  to 
vary  the  speed  profile  independently.  With  an 
abstract  argument  it  becomes  possible  via 
introducing  the  speed  factor  A  such  that 

(i4) 

at 


Now  instead  of  (16)  we  have 


V(t)  =  Mt)Jx'(t)2+z'(t)2  (15) 

and  by  varying  A(r)  can  achieve  any  speed  profile 
we  want.  Therefore  using  a  virtual  arc  becomes  a  key 
element  in  the  proposed  approach. 

All  together  the  robustness  of  the  approach  allows 
generating  the  trajectory  to  accommodate  for  sudden 
changes  like  newly  discovered  obstacles.  As  an 
example  Fig.  6  demonstrates  the  scenario  when  an 
AUV  avoiding  the  first  obstacle  discovers  that  there 
is  another  one  right  behind  the  first  and  to  avoid  the 
collision  a  new  trajectory  starting  exactly  with  the 
current  states  and  control  (up  to  the  second-order 
derivatives  of  the  states)  needs  to  be  generated.  The 
method  allows  doing  this  and  assures  smooth  non¬ 
shock  transition. 


x=wsm0+ucos0 

z=wccs0-usm6> 

9=q 

vv=  \  {w+A^2q+Aa6+  BtSs 
Q—A1iW+Anq+As6+B28s 


x'=A  \wsm9+uces0) 
z' =A~\wccs9-usm9) 
d=X\ 

m> =X\Alxw+Allq+A[lid+Bx8A 
</  = A  1  ( Allw+  A^^q + An9+  BjiA) 

(17) 


We  will  further  use  a  slightly  modified  scheme  and 
having  the  boundary  conditions  needed  to  be 
satisfied  and  some  initial  guesses  on  the  varied  (free) 
parameters  we  only  establish  a  reference  polynomial 
for  the  vertical  coordinate  z(r)  (13).  Now  we 
proceed  with  a  numerical  procedure  for  computing 
all  remaining  states  along  the  reference  trajectory 
over  a  fixed  set  of  N  points  (for  instance,  V=100) 
equidistantly  placed  along  the  virtual  arc  [0;r,]  with 

the  interval  of 

Ar  (18) 

N-l 


We  take  the  very  next  value  for  the  virtual  arc 

Tj=TJ_l  +  Ar  j  =  2,...,N  (19) 

and  compute  the  corresponding  parameters  of  the 
trajectory  from  pAr)  and  p.Az)  (13).  Having  these, 
we  proceed  with  computing  the  corresponding  time 
period  At  . and  elapsed  time  t .  : 


8.  RESULTS 


A/, 


•7-1 


(20) 


tj=tJ_  1+A^!  (t,=0) 


Next  we  compute  the  current  value  of  the  speed 
factor: 


2,=-^  (2,=^  =  1.5)  (21) 

A  0-, 

Having  those  we  can  exploit  two  of  the  equations  in 
the  virtual  domain  (20)  as  follows: 

x'j- 1  =  tf-x  ( Wj- 1  sin  0 +  m0  cos  0M ) 

Wy-1  —  2-y-l  (AlWy_l  +  ^12? J  — 1  "I"  ■*"  )  (22) 

x  -  =  xy._[  +  Arx'j 
w.  =  w,  ,  +  Arw'_j 

The  next  step  is  to  compute  the  pitch  angle,  pitch  rate 
and  pitch  acceleration: 


9 .  =  cos  1 


UQXj-\  +  WjZj 
7  2  2 

V  Wy+M0 


■  <9,  -  0,  1 


At 


(23) 


y-i 


Demonstrations  were  recently  completed  in  an 
exercise  sponsored  by  the  Office  of  Naval  Research 
in  Panama  City  FL.  The  obstacle  was  a  sunken  60m 
oil  supply  ship.  At  its  highest  point  the  vessel  was 
approximately  12  meters  off  the  ocean  floor.  Runs 
were  made  at  the  vessel  perpendicular  to  the 
direction  of  the  keel.  The  optimal  trajectory  planner 
was  not  implemented  on  these  runs.  Figure  7  shows  a 
side  scan  sonar  image  which  shows  the  response  of 
the  AUV  to  the  sunken  ship. 


Figure  7.  Side  scan  sonar  image  from  the  AUV 
during  an  reactive  avoidance  manuever. 


Finally,  we  compute  the  dive  plane  deflection 
required  to  follow  the  trajectory 

8,-J  =  1 -  Ai Wj  -  A22 <lj  -  A23 Oj)  (24) 

7.3  Optimization 

When  all  parameters  (states  and  control)  are 
computed  in  each  of  N  points,  we  can  compute  the 
performance  index  and  the  penalty  function. 
Specifically,  according  to  (10),  the  penalty  function 
can  be  formed  as  follows: 


A  = 


min  (0;  z(xobs )  —  zobs  )2 
max  (°;  1^1  —  ^) 
mfx(0;|A,;|-7max) 


(25) 


where  the  penalties  on  violation  of  constraints  on 
controls  and  their  derivatives  are  scaled  with  the 
corresponding  factors.  Now  the  problem  can  be 
solved  say  in  the  MATLAB  development 
environment  with  the  built-in  fmincon  function  or 
fminsearch  function.  In  the  latter  case  we  have  to 
combine  the  performance  index  and  the  penalty 
function  together. 
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